/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "UPRE_AirCylinder.h"
#include "UPRE_Decision.h"
#include "UPRE_EX_RUPT.h"
#include "usart.h"
#include "UPRE_MOVEBASE.h"
#include "UPRE_MOTOR.h"
#include "UPRE_LOCATION.h"
#include "stm32f4xx_it.h"
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_GYRO100A.h"
#include "UPRE_PATH.h"
#include "UPRE_Hand_Shank.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for Task_Main */
osThreadId_t Task_MainHandle;
const osThreadAttr_t Task_Main_attributes = {
  .name = "Task_Main",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityRealtime7,
};
/* Definitions for Task_Exchange */
osThreadId_t Task_ExchangeHandle;
const osThreadAttr_t Task_Exchange_attributes = {
  .name = "Task_Exchange",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Task_Rupt */
osThreadId_t Task_RuptHandle;
const osThreadAttr_t Task_Rupt_attributes = {
  .name = "Task_Rupt",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for Task_Gyro */
osThreadId_t Task_GyroHandle;
const osThreadAttr_t Task_Gyro_attributes = {
  .name = "Task_Gyro",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void Start_Main(void *argument);
void Start_Exchange(void *argument);
void Start_Rupt(void *argument);
void Start_Gyro(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of Task_Main */
  Task_MainHandle = osThreadNew(Start_Main, NULL, &Task_Main_attributes);

  /* creation of Task_Exchange */
  Task_ExchangeHandle = osThreadNew(Start_Exchange, NULL, &Task_Exchange_attributes);

  /* creation of Task_Rupt */
  Task_RuptHandle = osThreadNew(Start_Rupt, NULL, &Task_Rupt_attributes);

  /* creation of Task_Gyro */
  Task_GyroHandle = osThreadNew(Start_Gyro, NULL, &Task_Gyro_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_Start_Main */
/**
  * @brief  Function implementing the Task_Main thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_Start_Main */
void Start_Main(void *argument)
{
  /* USER CODE BEGIN Start_Main */
  /* Infinite loop */
  for(;;)
  {
      Task_Decision();
      osDelay(1);
  }
  /* USER CODE END Start_Main */
}

/* USER CODE BEGIN Header_Start_Exchange */
/**
* @brief Function implementing the Task_Exchange thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Exchange */
void Start_Exchange(void *argument)
{
  /* USER CODE BEGIN Start_Exchange */

  /* Infinite loop */
  for(;;)
  {
      Start_Hand_Shank_Receive();
      osDelay(1);
  }
  /* USER CODE END Start_Exchange */
}

/* USER CODE BEGIN Header_Start_Rupt */
/**
* @brief Function implementing the Task_Rupt thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Rupt */
void Start_Rupt(void *argument)
{
  /* USER CODE BEGIN Start_Rupt */
  /* Infinite loop */
  for(;;)
  {
    Encoder_On();
    cal_location();
    robotMoveTask();
    GoBesselTask1();
    osDelay(1);
  }
  /* USER CODE END Start_Rupt */
}

/* USER CODE BEGIN Header_Start_Gyro */
/**
* @brief Function implementing the Task_Gyro thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_Gyro */
void Start_Gyro(void *argument)
{
  /* USER CODE BEGIN Start_Gyro */
  /* Infinite loop */
  for(;;)
  {
//      Motor_ClearPID();

      Motor_control(Motor_Set_param.posi_arml,Motor_Set_param.posi_armr,Motor_Set_param.vel_daj,Motor_Set_param.vel_vesc_trans,Motor_Set_param.vesc_shoot);
      Time_Stamp();
      EX_Task_change();
      osDelay(1);
  }
  /* USER CODE END Start_Gyro */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */

/* USER CODE END Application */

